package Estados;

import teams.ucmTeam.RobotAPI;
import EDU.gatech.cc.is.util.Vec2;
import MaquinaEstados.Estado;
import MaquinaEstados.MaquinaEstados;

public class EstadoAtacanteAtacando2 extends Estado{
	
	public EstadoAtacanteAtacando2(MaquinaEstados miMaquina) {
		super(miMaquina);
	}

	@Override
	protected void onEnd(RobotAPI robot) {
		robot.setDisplayString("");	
	}

	@Override
	protected void onInit(RobotAPI robot) {
		robot.setDisplayString("AtacanteAtacando2");	
	}

	@Override
	protected void onTakeStep(RobotAPI robot) {
		boolean closestToBall = robot.closestToBall();
		boolean aligned = robot.alignedToBallandGoal();
		int campo = robot.getFieldSide();
		Vec2 ball= robot.getBall();	
		Vec2 goal = robot.getOpponentsGoal();
		Vec2 oponent = robot.getClosestOpponent();

		if (closestToBall){
			if (ball.r<0.1) {//Si estoy pegado miro a la porteria rival
				robot.setBehindBall(robot.getOpponentsGoal());
				robot.setSpeed(1.0);
				if ((goal.r<0.3) || (oponent.r<0.1)){ //se deshace del balon tirando
					robot.setSteerHeading(robot.getOpponentsGoal().t);
					if(robot.canKick()){
						robot.kick();
					}
				}
			}else { //miro al balon
				robot.setBehindBall(robot.getBall());
				robot.setSpeed(1.0);
			}
			

	
		}//Si no es el  mas cercano,pero esta cerca, va hacia la bola
		else
			if (ball.r<0.4) {
			robot.setBehindBall(robot.getOpponentsGoal());
			if(robot.canKick())
				robot.kick(); 
			}

			//Si no es el  mas cercano, y esta lejos de la bola, va al medio campo
			else{
				double x = 0.7 * campo * -1;
				double y = ball.y;
				//Vec2 destino = robot.getClosestOpponent();
				Vec2 destino = new Vec2(x, y);
				Vec2 destEgo = robot.toEgocentricalCoordinates(destino);
				double t=destEgo.t;
				robot.setSteerHeading(t);
				robot.setSpeed(1.0);
				Vec2 dist = destEgo;
				dist.sub(robot.getPosition());
				if (dist.r<0.2){
					robot.setSpeed(0.0);
				}
				//robot.blockClosest();
			}
	}
}
